#ifndef __IS_CORE_RAYTRACE_INTERSECTTRIANGLE__
#define __IS_CORE_RAYTRACE_INTERSECTTRIANGLE__

namespace IS
{

// source code copied from:
// Fast, Minimum Storage Ray/Triangle Intersection
// http://www.cs.virginia.edu/~gfx/Courses/2003/ImageSynthesis/papers/Acceleration/Fast%20MinimumStorage%20RayTriangle%20Intersection.pdf
//
#define __EPSILON__ 0.000001

#define __CROSS__(dest,v1,v2) \
	dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
	dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
	dest[2]=v1[0]*v2[1]-v1[1]*v2[0];

#define __DOT__(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])

#define __SUB__(dest,v1,v2) \
	dest[0]=v1[0]-v2[0]; \
	dest[1]=v1[1]-v2[1]; \
	dest[2]=v1[2]-v2[2];

template<typename REAL>
int intersect_triangle(
	const REAL orig[3], const REAL dir[3],
	const REAL vert0[3], const REAL vert1[3], const REAL vert2[3],
	REAL *t, REAL *u, REAL *v,
	bool twosided)
{
	REAL edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
	REAL det,inv_det;
	// find vectors for two edges sharing vert0
	__SUB__(edge1, vert1, vert0);
	__SUB__(edge2, vert2, vert0);
	// begin calculating determinant - also used to calculate U parameter
	__CROSS__(pvec, dir, edge2);
	// if determinant is near zero, ray lies in plane of triangle
	det = __DOT__(edge1, pvec);

	if(!twosided) 
	{	// define TEST_CULL if culling is desired
		if (det < __EPSILON__)
			return 0;
		// calculate distance from vert0 to ray origin
		__SUB__(tvec, orig, vert0);
		// calculate U parameter and test bounds
		*u = __DOT__(tvec, pvec);
		if (*u < 0.0 || *u > det)
			return 0;
		// prepare to test V parameter
		__CROSS__(qvec, tvec, edge1);
		// calculate V parameter and test bounds
		*v = __DOT__(dir, qvec);
		if (*v < 0.0 || *u + *v > det)
			return 0;

		// calculate t, scale parameters, ray intersects triangle
		*t = __DOT__(edge2, qvec);
		inv_det = (REAL)1.0 / det;
		*t *= inv_det;
		*u *= inv_det;
		*v *= inv_det;
	}
	else
	{	// the non-culling branch
		if (det > (REAL)-__EPSILON__ && det < (REAL)__EPSILON__)
			return 0;
		inv_det = (REAL)1.0 / det;
		// calculate distance from vert0 to ray origin
		__SUB__(tvec, orig, vert0);
		// calculate U parameter and test bounds
		*u = __DOT__(tvec, pvec) * inv_det;
		if (*u < (REAL)0.0 || *u > (REAL)1.0)
			return 0;
		// prepare to test V parameter
		__CROSS__(qvec, tvec, edge1);
		// calculate V parameter and test bounds
		*v = __DOT__(dir, qvec) * inv_det;
		if (*v < (REAL)0.0 || *u + *v > (REAL)1.0)
			return 0;
		// calculate t, ray intersects triangle
		*t = __DOT__(edge2, qvec) * inv_det;
	}

	return 1;
}

#undef __EPSILON__
#undef __CROSS__
#undef __DOT__
#undef __SUB__

template<typename REAL>
int intersectTriangle(
	const Vec3<REAL>& orig, const Vec3<REAL>& dir,
	const Vec3<REAL>& vert0, const Vec3<REAL>& vert1, const Vec3<REAL>& vert2,
	REAL *t, REAL *u, REAL *v,
	bool twosided)
{
	return intersect_triangle<REAL>(orig.data, dir.data, vert0.data, vert1.data, vert2.data, t, u, v, twosided);
}

}	// namespace IS

#endif  // __IS_CORE_RAYTRACE_INTERSECTTRIANGLE__